
#include <fstream>
#include <ios>
#include <sys/types.h> 
#include <sys/stat.h>

#include "../../Header/FSM/cFSMGPU.h"
#include "../../Header/ShaderObject.h"
#include "../../Header/framebufferObject.h"
#include "../../Header/glErrorUtil.h"
#include "../../Header/misc.h"
//#include "../../Header/misc/nvmath.h"

#ifndef RAND0to1
	#define RAND0to1 (rand()*1.0f/(RAND_MAX-1.0f))
#endif RAND0to1


cFSMGPU::cFSMGPU ()
{
	int x;
	float *imgFloat;
	float theta = 3.14159/4.0;
	//nv::matrix4f rotY(cos(theta), 0, sin(theta), 0, 0, 1, 0, 0, -sin(theta), 0, cos(theta), 0, 0, 0, 0, 1);
	
	passthru = new ShaderObject("shaders/FSM/passthruVertex.glsl","shaders/FSM/passthruFragment.glsl");
	passthru->activate();
		passthru->setUniformi("tex",0);
	passthru->deactivate();
	
	fsmSimpleShader = new ShaderObject("shaders/FSM/passthruVertex.glsl","shaders/FSM/fsmSimpleFragment.glsl");
	fsmSimpleShader->activate();
		fsmSimpleShader->setUniformi("agentsPos", 0);
		fsmSimpleShader->setUniformi("fsm", 1);
		fsmSimpleShader->setUniformi("soilLabels",2);
	fsmSimpleShader->deactivate();
		

	BmpImage *img = new BmpImage ();
	
	numAgentsWidth = 2048;
	numAgentsHeight = 2048;
	x=0;
	int i,j;
	imgFloat = new float [numAgentsWidth*numAgentsHeight*4];
	for (i=0;i<numAgentsWidth;i++)
		for (j=0;j<numAgentsHeight;j++)
		{
			//nv::vec4f pos (RAND0to1*numAgentsWidth*4.0, 0.0, RAND0to1*numAgentsHeight, 1.0);
			//nv::vec4f rotPos = rotY*pos;
			imgFloat[x] = RAND0to1*numAgentsWidth/2.0;
			imgFloat[x+1] = RAND0to1*numAgentsHeight/2.0;
			imgFloat[x+2]=0.0;
			imgFloat[x+3]=0.0;
			x+=4;
		}
	agentsFBO = new FramebufferObject();
	CheckErrorsGL("agentsFBO  BEGIN : Creating textures");
	agentsFBO->Bind();
		glGenTextures (1, &texIdFBO[cFSMGPU::POSITIONS_ID]);
		glBindTexture (GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::POSITIONS_ID]);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);			
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB, numAgentsWidth, numAgentsHeight, 0, GL_RGBA, GL_FLOAT, imgFloat);

		glGenTextures (1, &texIdFBO[cFSMGPU::POSITIONS_NEXT_ID]);
		glBindTexture (GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::POSITIONS_NEXT_ID]);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);			
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGBA32F_ARB, numAgentsWidth, numAgentsHeight, 0, GL_RGBA, GL_FLOAT, 0);

		CheckErrorsGL("agentsFBO END : Creating textures");	
		CheckErrorsGL("agentsFBO BEGIN : Configuring FBO");
		agentsFBO->AttachTexture(GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::POSITIONS_NEXT_ID], GL_COLOR_ATTACHMENT0_EXT);
		agentsFBO->AttachTexture(GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::POSITIONS_ID], GL_COLOR_ATTACHMENT1_EXT);
		agentsFBO->IsValid();
	agentsFBO->Disable ();
	CheckErrorsGL("agentsFBO BEGIN : Configuring FBO");

	
	agentsRTV	= new RenderVertexArray (numAgentsWidth*numAgentsHeight, 4, GL_FLOAT);

	FREE_ARRAY(imgFloat);

	img->readBmpFile ("FSMData/labelIds.bmp");	
	int m_w = img->width;
	int m_h = img->height;


	imgFloat = new float [m_w*m_h*3];
	for (x=0;x<m_w*m_h*3;x++)
		imgFloat[x] = img->data[x]*1.0;

	CheckErrorsGL("cFSMGPU::LABELS_ID  BEGIN : Creating textures");
		glGenTextures (1, &texIdFBO[cFSMGPU::LABELS_ID]);
		glBindTexture (GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::LABELS_ID]);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);			
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGB32F_ARB, m_w, m_h, 0, GL_RGB, GL_FLOAT, imgFloat);
	CheckErrorsGL("cFSMGPU::LABELS_ID  END : Creating textures");


	FREE_ARRAY(imgFloat);

/*
	img->readBmpFile ("FSMData/test1.bmp");	
	m_w = img->width;
	m_h = img->height;
	unsigned int texId2;

	glGenTextures (1, &texId2);
	glBindTexture (GL_TEXTURE_2D, texId2);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_REPEAT);
	glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_REPEAT);
	glTexImage2D  (GL_TEXTURE_2D, 0, GL_RGB, m_w, m_h, 0, GL_RGB, GL_UNSIGNED_BYTE, img->data);



	glNewList (ID_LIST_SOIL, GL_COMPILE);
		glColor3f (1,1,1);
		glEnable (GL_TEXTURE_2D);
		glActiveTexture(GL_TEXTURE0);
		glBindTexture (GL_TEXTURE_2D, texId2);
		glBegin (GL_QUADS);
	    glTexCoord2f(0,         0);         
		glVertex3fv(floorVertices[0]);
	    glTexCoord2f(16, 0);         
		glVertex3fv(floorVertices[1]);
		glTexCoord2f(16,16); 
		glVertex3fv(floorVertices[2]);
		glTexCoord2f(0, 16); 
		glVertex3fv(floorVertices[3]);
		glEnd ();
	glEndList ();
*/
	

	img->readBmpFile ("FSMData/fsm2.bmp");	
	m_w = img->width;
	m_h = img->height;

	imgFloat = new float [m_w*m_h*3];
	for (x=0;x<m_w*m_h*3;x++)
		imgFloat[x] = img->data[x]*1.0;


	CheckErrorsGL("cFSMGPU::FSM_ID  BEGIN : Creating textures");
		glGenTextures (1, &texIdFBO[cFSMGPU::FSM_ID]);
		glBindTexture (GL_TEXTURE_RECTANGLE_ARB, texIdFBO[cFSMGPU::FSM_ID]);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_S, GL_CLAMP);
		glTexParameteri(GL_TEXTURE_RECTANGLE_ARB, GL_TEXTURE_WRAP_T, GL_CLAMP);			
		glTexImage2D(GL_TEXTURE_RECTANGLE_ARB, 0, GL_RGB32F_ARB, m_w, m_h, 0, GL_RGB, GL_FLOAT, imgFloat);
	CheckErrorsGL("cFSMGPU::FSM_ID  END : Creating textures");

	FREE_ARRAY(imgFloat);



}
/*------------------------------------------------------------------------------------------------*/
cFSMGPU::~cFSMGPU ()
{
	FREE_MEMORY(passthru);
	FREE_MEMORY(fsmSimpleShader);

	FREE_MEMORY(agentsFBO);
	
	FREE_MEMORY(agentsRTV);

	glDeleteLists (ID_LIST_SOIL, 1);
}

unsigned int cFSMGPU::calculateState (float fps)
{
	/*
	float deltaTime;
	if (fps<=1.0)
		deltaTime = 1.0 / 30.0;
	else
		deltaTime = 1.0 / fps;
*/
	CheckErrorsGL(" cFSMGPU::calculateState  BEGIN : Creating textures");
	agentsFBO->Bind();
		glDrawBuffer (GL_COLOR_ATTACHMENT0_EXT);
		setFBOProjection(numAgentsWidth, numAgentsHeight);
			glActiveTexture(GL_TEXTURE0);
			glBindTexture(GL_TEXTURE_RECTANGLE_NV, texIdFBO[cFSMGPU::POSITIONS_ID]);
			
			glActiveTexture(GL_TEXTURE1);
			glBindTexture(GL_TEXTURE_RECTANGLE_NV, texIdFBO[cFSMGPU::FSM_ID]);
			
			glActiveTextureARB(GL_TEXTURE2);
			glBindTexture(GL_TEXTURE_RECTANGLE_NV, texIdFBO[cFSMGPU::LABELS_ID]);

			fsmSimpleShader->activate();
				fsmSimpleShader->setUniformf("deltaTime", fps);
				DrawQuad(numAgentsWidth, numAgentsHeight, numAgentsWidth, numAgentsHeight);
			fsmSimpleShader->deactivate();

			glBindTexture(GL_TEXTURE_RECTANGLE_NV, 0);
			glActiveTexture(GL_TEXTURE1);
			glBindTexture(GL_TEXTURE_RECTANGLE_NV, 0);
			glActiveTexture(GL_TEXTURE0);
			glBindTexture(GL_TEXTURE_RECTANGLE_NV, 0);
		
			
		glDrawBuffer (GL_COLOR_ATTACHMENT1_EXT);
		glActiveTexture(GL_TEXTURE0);
		glBindTexture(GL_TEXTURE_RECTANGLE_NV, texIdFBO[cFSMGPU::POSITIONS_NEXT_ID]);
		passthru->activate ();
			DrawQuad(numAgentsWidth, numAgentsHeight, numAgentsWidth, numAgentsHeight);
		passthru->deactivate ();
		restoreProjection();

		agentsRTV->Read (GL_COLOR_ATTACHMENT0_EXT, numAgentsWidth,numAgentsHeight);

	agentsFBO->Disable();

	//return agentsRTV->getVBOId();
	return texIdFBO[cFSMGPU::POSITIONS_ID];
}

void cFSMGPU::setFBOProjection (int w, int h)
{
	
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	glGetIntegerv(GL_VIEWPORT, vp);
	glViewport (0,0, w, h);
	glOrtho(0.0, w, 0.0, h, -1,1);
	glMatrixMode (GL_MODELVIEW);
	glPushMatrix ();
	glLoadIdentity ();
}

void cFSMGPU::restoreProjection ()
{
	glPopMatrix ();
	glMatrixMode(GL_PROJECTION);
	glViewport (vp[0],vp[1], vp[2], vp[3]);
	glPopMatrix();
	glMatrixMode (GL_MODELVIEW);
}

void cFSMGPU::DisplayTexture (unsigned int texId, int w, int h)
{
	glBindTexture(GL_TEXTURE_RECTANGLE_ARB, texId);
	passthru->activate ();
		DrawQuad(w, h, w, h);
	passthru->deactivate ();

}


void cFSMGPU::DisplayTextures ()
{
	glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glOrtho(0.0, 128, 0.0, 128, -1.0, 1.0);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

	glScalef(0.5f, 0.5f, 1.0f);	 
    	
	//DisplayTexture(agents, WGL_FRONT_LEFT_ARB, agents->GetWidth(), agents->GetHeight());

	DisplayTexture(texIdFBO[cFSMGPU::POSITIONS_ID], 128, 128);
	/*
	glTranslatef(agents->GetWidth(), 0.0f, 0.0f);
	DisplayTexture(soilLabels, WGL_FRONT_LEFT_ARB, soilLabels->GetWidth (), soilLabels->GetHeight());

	glTranslatef(soilLabels->GetWidth(), 0.0f, 0.0f);
	glScalef (30,30,0);

	DisplayTexture(FSM, WGL_FRONT_LEFT_ARB, FSM->GetWidth (), FSM->GetHeight());
*/
	glPopMatrix();

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);

}
 
void cFSMGPU::DrawQuad (int w, int h, int tw, int th)
{
    glBegin(GL_QUADS);
/*
	glMultiTexCoord2f (GL_TEXTURE0_ARB, 0,0); 
	glMultiTexCoord2f (GL_TEXTURE1_ARB, 0,0); 
	glVertex2f(0,        0);

	glMultiTexCoord2f (GL_TEXTURE0_ARB, (float)tw,0); 
	glMultiTexCoord2f (GL_TEXTURE1_ARB, (float)tw,0); 
	glVertex2f((float)w, 0);

	glMultiTexCoord2f (GL_TEXTURE0_ARB, (float)tw, (float)th); 
	glMultiTexCoord2f (GL_TEXTURE1_ARB, (float)tw, (float)th); 
	glVertex2f((float)w, (float) h);

	glMultiTexCoord2f (GL_TEXTURE0_ARB, 0,         (float)th); 
	glMultiTexCoord2f (GL_TEXTURE1_ARB, 0,         (float)th); 
	glVertex2f(0,         (float)h);
*/	
    glTexCoord2f(0,         0);         glVertex2f(0,        0);
    glTexCoord2f((float)tw, 0);         glVertex2f((float)w, 0);
    glTexCoord2f((float)tw, (float)th); glVertex2f((float)w, (float) h);
    glTexCoord2f(0,         (float)th); glVertex2f(0,        (float) h);

	glEnd();
}


void cFSMGPU::showAgentsInstanced ()
{
	glCallList(ID_LIST_SOIL);
}

void cFSMGPU::showAgents ()
{

	glCallList (ID_LIST_SOIL);
	/*

	CheckErrorsGL(" cFSMGPU::showAgents () BEGIN : Creating textures");
	agentsFBO->Bind();
		agentsRTV->Read (GL_COLOR_ATTACHMENT0_EXT, numAgentsWidth,numAgentsHeight);
	agentsFBO->Disable ();
	CheckErrorsGL(" cFSMGPU::showAgents () END : Creating textures");

		

	agentsRTV->SetPointer (0);
	glEnableVertexAttribArrayARB(0);
	passthru->activate();		
		glDrawArrays(GL_POINTS,0,numAgentsWidth*numAgentsHeight);
	passthru->deactivate();
	glDisableVertexAttribArrayARB(0);

*/
}
